#include <Servo.h>

#include <PS2X_lib.h>
#include "State.h"
#define CLK_PIN 13
#define CMD_PIN 11
#define ATN_PIN 10
#define DAT_PIN 12
#define SERVO_PIN 7

PS2X ps2x;
int error = 0; 
byte type = 0;

State currentState;
State lastState;
Servo servo;
void setup(){
  memset(&currentState, 0, sizeof(State));
  Serial.begin(57600);
  Serial3.begin(57600);
  servo.attach(SERVO_PIN);
  servo.write(90);

  //setup pins and settings:  GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error
  error = ps2x.config_gamepad(CLK_PIN, CMD_PIN, ATN_PIN, DAT_PIN, true, true);

  if(error == 0){
    Serial.println("Found Controller, configured successful");
  } 
  else if(error == 1){
    Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");
  } 
  else if(error == 2){
    Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");
  } 
  else if(error == 3){
    Serial.println("Controller refusing to enter Pressures mode, may not support it. ");
  }

  type = ps2x.readType(); 

  switch(type) {
  case 0:
    Serial.println("Unknown Controller type");
    break;
  case 1:
    Serial.println("DualShock Controller Found");
    break;
  case 2:
    Serial.println("GuitarHero Controller Found");
    break;
  }
  delay(1000);
}

void loop(){
  if(error == 1){ 
    return; 
  }
  else { 
    lastState = currentState;
    ps2x.read_gamepad();
    currentState.SelectButton = ps2x.Button(PSB_SELECT);
    currentState.StartButton  = ps2x.Button(PSB_START);

    currentState.LeftStickX  = ps2x.Analog(PSS_LX);
    currentState.LeftStickY  = ps2x.Analog(PSS_LY);
    currentState.RightStickX = ps2x.Analog(PSS_RX);
    currentState.RightStickY = ps2x.Analog(PSS_RY);

    currentState.PressureUp    = ps2x.Analog(PSAB_PAD_UP);
    currentState.PressureRight = ps2x.Analog(PSAB_PAD_RIGHT);
    currentState.PressureDown  = ps2x.Analog(PSAB_PAD_DOWN);
    currentState.PressureLeft  = ps2x.Analog(PSAB_PAD_LEFT);

    currentState.PressureL1 = ps2x.Analog(PSAB_L1);
    currentState.PressureR1 = ps2x.Analog(PSAB_R1);
    currentState.PressureL2 = ps2x.Analog(PSAB_L2);
    currentState.PressureR2 = ps2x.Analog(PSAB_R2);

    currentState.PressureTriangle = ps2x.Analog(PSAB_TRIANGLE);
    currentState.PressureCircle   = ps2x.Analog(PSAB_CIRCLE);
    currentState.PressureCross    = ps2x.Analog(PSAB_CROSS);
    currentState.PressureSquare   = ps2x.Analog(PSAB_SQUARE);


    if(lastState != currentState){
      currentState.printDiff(lastState);
      Serial.print("--------------------------------\n");
      int angulo = map(currentState.LeftStickX, 0, 255, 0, 179);
      Serial.println(angulo);
      servo.write(angulo);
    }
  }
  delay(50);
}






